module FES2DT63TH_m
use AFES_m
use Mesh2DT3_m
use Mesh2DT6_m
use FES2DT31_m
use FES2DT61_m
type, extends(AFES) :: FES2DT63TH
    type(Mesh2DT6), pointer :: meshVel
    type(Mesh2DT3), pointer :: meshPre
contains
    procedure :: initFES => initFES2DT63TH
    procedure :: valueFromFunc2DT6
    procedure :: valueFromFunc2DT3
    procedure :: assembleStokes2DT63TH_Const
    generic :: assembleStokes => assembleStokes2DT63TH_Const
    
    procedure :: assembleSource2DT63TH_function
    procedure :: assembleSource2DT63TH_const
    generic :: assembleSource => assembleSource2DT63TH_function, assembleSource2DT63TH_const
    
    procedure :: applyBCZero2DT63TH_MR
    generic :: applyBCZero => applyBCZero2DT63TH_MR
    
    procedure :: applyBC2DT63TH_function
    procedure :: applyBC2DT63TH_const
    generic :: applyBC => applyBC2DT63TH_function, applyBC2DT63TH_const
    
    procedure :: extractUVP => extractUVP2DT63TH
end type
contains

subroutine initFES2DT63TH(this, meshT6, meshT3)
    implicit none
    class(FES2DT63TH), intent(out) :: this
    type(Mesh2DT6), intent(in), target :: meshT6
    type(Mesh2DT3), intent(in), target :: meshT3
    
    this.meshVel => meshT6
    this.meshPre => meshT3
    this.dim = 2
    this.nDof = 2 * meshT6.nv + meshT3.nv + 1
    this.nVertices = meshT6.nv + meshT3.nv
    this.nElement = meshT6.nt
    this.nBoundary = meshT6.nb
    this.nPerElement = 9
    this.nPerBoundary = 3
    this.dofPerNode = 2
    this.dofPerElement = 15
    this.dofPerBoundary = 6
    this.nBand = 2 * meshT6.nv
end subroutine

subroutine valueFromFunc2DT6(this, func, u)
    implicit none
    class(FES2DT63TH), intent(in) :: this
    real*8, external :: func
    real*8, intent(out) :: u(this.meshVel.nv)
    
    integer :: i
    type(Mesh2DT6), pointer :: mesh2D
    mesh2D => this%meshVel
    do i = 1, mesh2D%nv
        u(i) = func(mesh2D%nodes(i)%xy)
    end do
end subroutine

subroutine valueFromFunc2DT3(this, func, u)
    implicit none
    class(FES2DT63TH), intent(in) :: this
    real*8, external :: func
    real*8, intent(out) :: u(this.meshPre.nv)
    
    integer :: i
    type(Mesh2DT3), pointer :: mesh2D
    mesh2D => this%meshPre
    do i = 1, mesh2D%nv
        u(i) = func(mesh2D%nodes(i)%xy)
    end do
end subroutine


subroutine assembleStokes2DT63TH_Const(this, nu, A)
    implicit none
    class(FES2DT63TH), intent(in) :: this
    real*8, intent(in) :: nu
    type(SMatrix), intent(out) :: A

    integer :: i, j, nv, idx(6), idx2(3)
    real*8 :: xy(6, 2), nnu(6), dxP(6, 3), dyP(6, 3), K(6, 6), areaEle(3)
    real*8, allocatable :: areaT(:)
    integer :: areaIdx(3)
    type(mesh2DT6), pointer :: mesh2D
    type(mesh2DT3), pointer :: mesh2D2
    nnu = nu
    mesh2D => this%meshVel
    mesh2D2 => this%meshPre
    nv = mesh2D%nv
    allocate(areaT(mesh2D%nt))
    call mesh2D2.area(areaT)
    do i = 1, mesh2D%nt
        idx = mesh2D%elements(i)%idx
        idx2 = mesh2D2%elements(i)%idx
        xy(:, 1) = mesh2D%nodes(idx)%xy(1)
        xy(:, 2) = mesh2D%nodes(idx)%xy(2)
        call elementHeatStiff2DT61(xy, nnu, COMMON_2D, K)
        call elementPressPartialT3(xy, dxP, dyP)
        idx = mesh2D%elements(i)%idx
        call assembleMatrix(A, K, idx)
        call assembleMatrix(A, dxP, idx, idx2 + 2 * nv)
        call assembleMatrix(A, transpose(dxP), idx2 + 2 * nv, idx)
        idx = idx + nv
        call assembleMatrix(A, K, idx)
        call assembleMatrix(A, dyP, idx, idx2 + 2 * nv)       
        call assembleMatrix(A, transpose(dyP), idx2 + 2 * nv, idx)
        areaIdx = 2 * nv + idx2
        areaEle = areaT(i) / 3.0D0
        call assembleMatrixCol(A, areaEle, areaIdx, this%nDof)
        call assembleMatrixRow(A, areaEle, this%nDof, areaIdx)        
    end do
!    call assembleMatrixCol(A, areaT, areaIdx, this%nDof)
!    call assembleMatrixRow(A, areaT, this%nDof, areaIdx)
    deallocate(areaT)
end subroutine

subroutine elementPressPartialT3(xy, dxP, dyP)
    implicit none
    real*8, intent(in) :: xy(6, 2)
    real*8, intent(out) :: dxP(6, 3), dyP(6, 3)
    
    real*8 :: N(6), dxN(6), dyN(6), x(6), y(6)
    real*8 :: detJ, wxarea, L1, L2, L3
    integer :: i, sj
    x = xy(:, 1); y = xy(:, 2)
    
    !call gradientCR(x, y, dx, dy, area)
    !dxP = -dx * area
    !dyP = -dy * area
    dxP = 0.0D0
    dyP = 0.0D0
    do i = 1, 3
        L1 = TriPoints2(i, 1)
        L2 = TriPoints2(i, 2)
        L3 = TriPoints2(i, 3)
        detJ = P2Shape2(x, y, L1, L2, L3, N, dxN, dyN)
        wxarea = detJ * TriWeight2(i)
        dxP(:, 1) = dxP(:, 1) - dxN * L1 * wxarea
        dxP(:, 2) = dxP(:, 2) - dxN * L2 * wxarea
        dxP(:, 3) = dxP(:, 3) - dxN * L3 * wxarea
        dyP(:, 1) = dyP(:, 1) - dyN * L1 * wxarea
        dyP(:, 2) = dyP(:, 2) - dyN * L2 * wxarea
        dyP(:, 3) = dyP(:, 3) - dyN * L3 * wxarea
    end do
end subroutine

subroutine assembleSource2DT63TH_function(this, fu, fv, time, tp, RHS)
    implicit none
    class(FES2DT63TH), intent(in) :: this
    real*8, external :: fu, fv
    real*8, intent(in) :: time
    integer, intent(in) :: tp
    real*8, intent(inout) :: RHS(:)
    
    integer :: i, j, idx(6), label
    real*8 :: xy(6, 2), K(6), ffu(6), ffv(6)
    type(Mesh2DT6), pointer :: meshV
    meshV => this%meshVel
    do i = 1, meshV%nt
        idx = meshV%elements(i)%idx
        label = meshV%elements(i)%num
        xy(:, 1) = meshV%nodes(idx)%xy(1)
        xy(:, 2) = meshV%nodes(idx)%xy(2)
        do j = 1, 6
            ffu(j) = fu(xy(j, :), label, time)
            ffv(j) = fv(xy(j, :), label, time)
        end do
        call elementHeatSource2DT61(xy, ffu, tp, K)
        call assembleVector(RHS, K, idx)
        idx = idx + meshV%nv
        call elementHeatSource2DT61(xy, ffv, tp, K)
        call assembleVector(RHS, K, idx)
    end do
end subroutine

subroutine assembleSource2DT63TH_const(this, fu, fv, tp, RHS)
    implicit none
    class(FES2DT63TH), intent(in) :: this
    real*8, intent(in) :: fu, fv
    integer, intent(in) :: tp
    real*8, intent(inout) :: RHS(:)
    
    integer :: i, j, idx(6), label
    real*8 :: xy(6, 2), K(6), ffu(6), ffv(6)
    type(Mesh2DT6), pointer :: meshV
    meshV => this%meshVel
    ffu = fu
    ffv = fv
    do i = 1, meshV%nt
        idx = meshV%elements(i)%idx
        label = meshV%elements(i)%num
        xy(:, 1) = meshV%nodes(idx)%xy(1)
        xy(:, 2) = meshV%nodes(idx)%xy(2)
        call elementHeatSource2DT61(xy, ffu, tp, K)
        call assembleVector(RHS, K, idx)
        idx = idx + meshV%nv
        call elementHeatSource2DT61(xy, ffv, tp, K)
        call assembleVector(RHS, K, idx)
    end do
end subroutine

subroutine applyBCZero2DT63TH_MR(this, A, RHS, label, direct)
    implicit none
    class(FES2DT63TH), intent(in) :: this
    type(SMatrix), intent(inout) :: A
    real*8, intent(inout) :: RHS(:)
    integer, intent(in) :: label(:)
    integer, optional, intent(in) :: direct
    
    type(LinkedSet) :: dirichletBC
    call this.meshVel.extractBoundaryNodes(dirichletBC, label)
    if (present(direct)) then
        call this.applyBCZero1_MR(A, RHS, dirichletBC, direct)
    else
        call this.applyBCZero1_MR(A, RHS, dirichletBC)
    end if
	call dirichletBC.empty()
end subroutine

subroutine applyBC2DT63TH_function(this, A, RHS, label, u0, v0, time, tp, direct)
    use Set_m
    implicit none
    class(FES2DT63TH), intent(in) :: this
    type(SMatrix), intent(inout) :: A
    real*8, intent(inout) :: RHS(:)
    integer, intent(in) :: label(:), tp
    real*8, external :: u0, v0
    real*8, intent(in) :: time
    integer, intent(in), optional :: direct
    
    real*8, allocatable :: uu0(:)
    integer, allocatable :: BCDOF(:)
    integer :: i
    real*8 :: xy(2)
    type(Mesh2DT6), pointer :: meshV
    type(LinkedSet) :: BSet
    type(ele), pointer :: p
    meshV => this%meshVel
    
    call meshV.extractBoundaryNodes(BSet, label)
    
    if (present(direct)) then
        allocate(uu0(BSet%size))
        allocate(BCDOF(BSet%size))
        i = 0
        p => BSet%head
        if (direct == directX) then
            do while(associated(p))
                i = i + 1
                xy = meshV%nodes(p%n)%xy
                uu0(i) = u0(xy, time)
                BCDOF(i) = p%n
                p => p%next
            end do
        else if (direct == directY) then
            do while(associated(p))
                i = i + 1
                xy = meshV%nodes(p%n)%xy
                uu0(i) = v0(xy, time)
                BCDOF(i) = p%n + meshV%nv
                p => p%next
            end do
        end if
        call this.applyBC_DOF(A, RHS, BCDOF, uu0, tp)
    else
        allocate(uu0(2 * BSet%size))
        allocate(BCDOF(2 * BSet%size))
        i = 0
        p => BSet%head
        do while(associated(p))
            i = i + 1
            xy = meshV%nodes(p%n)%xy
            BCDOF(2 * i - 1) = p%n
            BCDOF(2 * i) = p%n + meshV%nv
            uu0(2 * i - 1) = u0(xy, time)
            uu0(2 * i) = v0(xy, time)
            p => p%next
        end do
        call this.applyBC_DOF(A, RHS, BCDOF, uu0, tp)
    end if
end subroutine

subroutine applyBC2DT63TH_const(this, A, RHS, label, u0, v0, tp, direct)
    use Set_m
    implicit none
    class(FES2DT63TH), intent(in) :: this
    type(SMatrix), intent(inout) :: A
    real*8, intent(inout) :: RHS(:)
    integer, intent(in) :: label(:), tp
    real*8, intent(in) :: u0, v0
    integer, intent(in), optional :: direct
    
    real*8, allocatable :: uu0(:)
    integer, allocatable :: BCDOF(:)
    integer :: i
    type(Mesh2DT6), pointer :: meshV
    type(LinkedSet) :: BSet
    type(ele), pointer :: p
    meshV => this%meshVel
    
    call meshV.extractBoundaryNodes(BSet, label)
    
    if (present(direct)) then
        allocate(uu0(BSet%size))
        allocate(BCDOF(BSet%size))
        i = 0
        p => BSet%head
        if (direct == directX) then
            uu0 = u0
            do while(associated(p))
                i = i + 1
                BCDOF(i) = p%n
                p => p%next
            end do
        else if (direct == directY) then
            uu0 = v0
            do while(associated(p))
                i = i + 1
                BCDOF(i) = p%n + meshV%nv
                p => p%next
            end do
        end if
        call this.applyBC_DOF(A, RHS, BCDOF, uu0, tp)
    else
        allocate(uu0(2 * BSet%size))
        allocate(BCDOF(2 * BSet%size))
        i = 0
        p => BSet%head
        do while(associated(p))
            i = i + 1
            BCDOF(2 * i - 1) = p%n
            BCDOF(2 * i) = p%n + meshV%nv
            uu0(2 * i - 1) = u0
            uu0(2 * i) = v0
            p => p%next
        end do
        call this.applyBC_DOF(A, RHS, BCDOF, uu0, tp)
    end if
end subroutine


subroutine extractUVP2DT63TH(this, x, u, v, p)
    implicit none
    class(FES2DT63TH), intent(in) :: this
    real*8, intent(in) :: x(this%nDof)
    real*8, intent(out) :: u(:)
    real*8, intent(out) :: v(:)
    real*8, intent(out) :: p(:)
    
    type(Mesh2DT6), pointer :: meshV
    type(Mesh2DT3), pointer :: meshP
    meshV => this%meshVel
    meshP => this%meshPre
    u = x(1:meshV%nv)
    v = x(meshV%nv+1:2 * meshV%nv)
    p = x(2 * meshV%nv + 1 : this%nDof - 1)
end subroutine


end module